home *** CD-ROM | disk | FTP | other *** search
/ MacHack 2000 / MacHack 2000.toast / pc / The Hacks / 3D QuickTime Dynamics / kSan Sources / PCMD.c < prev    next >
Encoding:
C/C++ Source or Header  |  2000-06-24  |  28.8 KB  |  851 lines
  1. #include "PCMD.h"
  2. #include "fp.h"
  3.  
  4. #include "kSanMethodPlugIn.h"
  5. #include "kSanSimulationPrivate.h"
  6. #include "VSList.h"
  7. #define bigVelocityErr -101
  8. #define smallVelocityErr -102
  9.  
  10. short  PCMDMethodConstructor (KozoDispatchStack *ds, constructorMethodArgs *cma);
  11. short   PCMDMethodDestructor (KozoDispatchStack *ds, destructorMethodArgs* dma);
  12. short  PCMDMethodDoMessage (KozoDispatchStack *ds, doMessageMethodArgs *dmma);
  13. void     PCMDDoCorrection(kozoObject *pcmdObj, PCMDMethod *pcmd, kSanSimulation *sim, particle *basePart,  kSanGroup **baseGroup, long numGroups, double  *delT);
  14. void    PCMDDoPrediction(kozoObject *pcmdObj, PCMDMethod *pcmd, kSanSimulation *sim, particle *basePart, double  *delT);
  15.  short PCMDDoCorrectionFreeGroups(kozoObject *pcmdObj, PCMDMethod *pcmd, kSanSimulation *sim, particle *basePart,  kSanGroup *gp, double  *delTPtr);
  16. short PCMDDoCorrectionBlockGroups(kozoObject *pcmdObj, PCMDMethod *pcmd, kSanSimulation *sim, particle *basePart,  kSanGroup *gp, double  *delTPtr);
  17. short PCMDDoCorrectionFixedGroups(kozoObject *pcmdObj, PCMDMethod *pcmd, kSanSimulation *sim, particle *basePart,  kSanGroup *gp, double  *delTPtr);
  18. short PCMDDoCorrectionMixedGroups(kozoObject *pcmdObj, PCMDMethod *pcmd, kSanSimulation *sim, particle *basePart,  kSanGroup *gp, double  *delTPtr);
  19. short PCMDIteration (  kSanMethodPlugIn *mpi,  kSanSimulation *sim) ;
  20. void    normalIteration(kozoObject *pcmdObj, PCMDMethod *pcmd, kSanSimulation *sim, particle *basePart, double  *delTPtr);
  21. short  PCMDMethodSaveSig(kozoObject *pcmdObj,  PCMDMethod *mpi);
  22. void   zeroDerivs(kozoObject *pcmdObj, PCMDMethod *pcmd);
  23. short  PCReallocate(kozoObject *pcmdObj, PCMDMethod *pcmd, kSanSimulation *sim);
  24. void   PCMDReleaseDerivs( PCMDMethod *pcmd);
  25. PCMDDerivatives *   PCMDLockDerivs(  PCMDMethod *pcmd);
  26.  
  27. short PCChangedInfo (kozoObject *pcmdObj, PCMDMethod *pcmd,  whatWasChangedStruct *wwc);
  28.  
  29. short initClassPCMDMethod(void)
  30. {
  31.     short err = noErr;
  32.     kozoClass *newClass;
  33.     kozoObject *co;
  34.  
  35.     newClass = addClass(&co,ClassPCMDMethod, ClassMethodPlugIn, sizeof (PCMDMethod), "PCMD method");
  36.     if (newClass)
  37.     {
  38.         err =    setFuncs(newClass, 0L,  PCMDMethodConstructor,
  39.         PCMDMethodDestructor, nilPointer, nilPointer, PCMDMethodDoMessage,
  40.          nilPointer, nilPointer, nilPointer, nilPointer);
  41.     }
  42.     return (err);
  43. }
  44. short  PCMDMethodConstructor (KozoDispatchStack *ds, constructorMethodArgs *cma)
  45. {
  46.     short err = noErr;
  47.     PCMDMethod *pcmd = nilPointer;
  48.     kSanMethodPlugIn *mpi = nilPointer;
  49.     kSanSimulation *sim = nilPointer;
  50.     
  51.      mpi = KDSGetPrivateData(ds, ClassMethodPlugIn );
  52.     
  53.     pcmd    = KDSGetPrivateData(ds, ClassPCMDMethod );
  54.  
  55.     VSListInitialize (&pcmd->derivs, sizeof(PCMDDerivatives));
  56.     
  57.     pcmd->alpha.nought   = 3.0 / 16.0;  
  58.     pcmd->alpha.one = 251.0 / 360.0;  
  59.     pcmd->alpha.two = 1.0;  
  60.     pcmd->alpha.three = 11.0 / 18.0;  
  61.     pcmd->alpha.four = 1.0 / 6.0;  
  62.     pcmd->alpha.five = 1.0 / 60.0;  
  63.     pcmd->dirtyDerivs = true;
  64.     mpi->methodPlugPrivateData = pcmd;
  65.     mpi->doIteration = PCMDIteration;
  66.     
  67.     
  68.     
  69.     err = dispatchLoop(ds, cma); // need this because of the get object in hierarchy
  70.     err =  kozoObjectGetObjectInHierarchy(cma->container, ClassSimulation , nilPointer, (void **)&sim);
  71.  
  72.       //err = dispatchMessageReport ( cma->container,  ClassKozoObject, kGetSimulationPtrMessage, typeSimulationPtrPtr,  &sim);
  73.     if (!err)
  74.     {
  75.         err = PCReallocate(ds->obj, pcmd, sim);
  76.     }
  77.     
  78.     KDSReturnError(ds,err);
  79. }
  80.  
  81. short  PCReallocate(kozoObject *pcmdObj, PCMDMethod *pcmd, kSanSimulation *sim)
  82. {
  83.     short err = noErr;
  84.     long numParts = 0;
  85.     dispatchCount(simOBJ(sim), ClassParticle, &numParts);
  86.     VSListExpandToSize(&pcmd->derivs, numParts);
  87.     if (!err) zeroDerivs(pcmdObj, pcmd);
  88.     return (err);
  89. }
  90.  
  91. void   PCMDReleaseDerivs( PCMDMethod *pcmd)
  92. {
  93.     VSListRelease(&pcmd->derivs);
  94. }
  95.  
  96. PCMDDerivatives *   PCMDLockDerivs( PCMDMethod *pcmd)
  97. {
  98.     PCMDDerivatives *baseDs;
  99.     baseDs = (PCMDDerivatives *) VSListLock(&pcmd->derivs);
  100.     return (baseDs);
  101. }
  102.  
  103. void   zeroDerivs(kozoObject *pcmdObj, PCMDMethod *pcmd)
  104. {
  105.     short err = noErr;
  106.     long partArrIndex;
  107.     long numParts = 0;
  108.     particle *basePart = nilPointer;
  109.     kSanSimulation *sim = nilPointer;
  110.     PCMDDerivatives zeroDeriv;
  111.     
  112.     zerokSanVectorD(&zeroDeriv.accel);
  113.     zerokSanVectorD(&zeroDeriv.thirdD);
  114.     zerokSanVectorD(&zeroDeriv.fourthD);
  115.     zerokSanVectorD(&zeroDeriv.fifthD);
  116.         
  117.     pcmd->derivs.howManyItems = 0;
  118.     err =  kozoObjectGetObjectInHierarchy(pcmdObj->container, ClassSimulation, nilPointer, (void **)&sim);
  119.     //err = dispatchMessageReport(pcmdObj, ClassMethodPlugIn, kGetSimulationPtrReport, typeSimulationPtrPtr, &sim);
  120.     if (!err)  
  121.     {
  122.         dispatchCount(simOBJ(sim), ClassParticle, &numParts);
  123.         basePart = simLockParts(sim);
  124.         for (partArrIndex = 0; partArrIndex<numParts; ++partArrIndex)
  125.         {
  126.             zeroDeriv.oldPosition = basePart->basePosition[partArrIndex];
  127.             VSListAdd(&pcmd->derivs, (Ptr) &zeroDeriv);
  128.         }
  129.         simReleaseParts(sim);
  130.     }
  131. }
  132. short   PCMDMethodDestructor (KozoDispatchStack *ds, destructorMethodArgs* dma)
  133. {
  134. #pragma unused(dma)
  135.     PCMDMethod *pcmd;
  136.     
  137.     pcmd    = KDSGetPrivateData(ds, ClassPCMDMethod );
  138.     VSListDispose (&pcmd->derivs);
  139.     KDSReturnContinue(ds);
  140. }
  141.  
  142. short  PCMDMethodDoMessage (KozoDispatchStack *ds, doMessageMethodArgs *dmma)
  143. {
  144.     short err = noErr;
  145.     PCMDMethod *pcmd;
  146.     
  147.     pcmd    = KDSGetPrivateData(ds, ClassPCMDMethod );
  148.     switch (dmma->message)
  149.     {
  150.         case kThermalizeMessage:
  151.         case kSetToZeroMessage:
  152.             dispatchLoop(ds, dmma);
  153.              pcmd->dirtyDerivs = true;
  154.             break;
  155.         case kSanChangedInfoMessage :
  156.             err = PCChangedInfo(ds->obj,pcmd, (whatWasChangedStruct *) dmma->data);
  157.             break;
  158.         default :
  159.             KDSReturnContinue(ds);
  160.     }
  161.     KDSDoneDispatch(ds);
  162.     KDSReturnError(ds, err);
  163. }
  164. short PCChangedInfo (kozoObject *pcmdObj, PCMDMethod *pcmd,  whatWasChangedStruct *wwc)  
  165. {
  166.     short err = noErr;
  167.     pcmd->dirtyDerivs = true;
  168.     if (wwc->numParts)
  169.     {
  170.         err =   PCReallocate(pcmdObj, pcmd, wwc->sim);
  171.     }
  172.     return (err);
  173. }
  174.  
  175. void    pcmdCalcInitialDerivs(kozoObject *pcmdObj, PCMDMethod *pcmd, kSanSimulation *sim, particle *basePart, double  *delTPtr);
  176. void    pcmdCalcInitialDerivs(kozoObject *pcmdObj, PCMDMethod *pcmd, kSanSimulation *sim, particle *basePart, double  *delTPtr)
  177. {
  178. //  the idea of this function is that when the derivatives are not initialized, sudden changes in the lower derivatives mean very big changes in
  179. // the higher derivatives.   By making guesses at the lower derivatives before initializing the higher derivatives, we reduce the 
  180. // initial error due to setting initial values for the higher derivatives
  181.     short err = noErr;
  182.     long count;
  183.     long i;
  184.      double delT;
  185.     PCMDDerivatives * baseDerivs;
  186.      baseDerivs = PCMDLockDerivs( pcmd);
  187.  
  188.      dispatchCount(simOBJ(sim), ClassParticle, &count);
  189.     // for delT / 8 predict second deriv;
  190.     for ( i = 0; i < count; ++i)
  191.     {  // set the higher derivatives to zero because their values are no good
  192.       // at this stage, we keep only the value for acceleration
  193.         zerokSanVectorD (&baseDerivs[i].accel);
  194.         zerokSanVectorD (&baseDerivs[i].thirdD);
  195.         zerokSanVectorD (&baseDerivs[i].fourthD);
  196.         zerokSanVectorD (&baseDerivs[i].fifthD);
  197.     }
  198.     delT = (*delTPtr) * 0.125; 
  199.     normalIteration(pcmdObj, pcmd, sim, basePart, &delT);
  200.  
  201.      for ( i = 0; i < count; ++i)
  202.     {  // set the higher derivatives to zero because their values are no good
  203.       // at this stage, we keep only the value for acceleration
  204.         zerokSanVectorD (&baseDerivs[i].thirdD);
  205.         zerokSanVectorD (&baseDerivs[i].fourthD);
  206.         zerokSanVectorD (&baseDerivs[i].fifthD);
  207.     }
  208.   // for delT / 4 predict third deriv;
  209.     delT = (*delTPtr) * 0.25; 
  210.     normalIteration(pcmdObj, pcmd, sim, basePart, &delT);
  211.     for ( i = 0; i < count; ++i)
  212.     {  // set the higher derivatives to zero 
  213.     //  because we guessed at the acceleration value last time, the value for third derivative should be good here
  214.         zerokSanVectorD (&baseDerivs[i].fourthD);
  215.         zerokSanVectorD (&baseDerivs[i].fifthD);
  216.     }
  217.   // for delT / 2 predict fourth deriv;
  218.     delT = (*delTPtr) * 0.5; 
  219.     normalIteration(pcmdObj, pcmd, sim, basePart, &delT);
  220.     for ( i = 0; i < count; ++i)
  221.     {  // set the higher derivatives to zero 
  222.     //  because we have reasonable values for second and third derivatives, the value for forth derivative here
  223.       // should be acceptable we guess that the fifth derivative will initially be zero.
  224.         zerokSanVectorD (&baseDerivs[i].fifthD);
  225.     }
  226.        pcmd->dirtyDerivs = false;
  227.  // predict initially fifth deriv = 0;
  228. }
  229. void    PCMDDoPrediction(kozoObject *pcmdObj, PCMDMethod *pcmd, kSanSimulation *sim, particle *basePart, double  *delTPtr)
  230. {
  231. #pragma unused(sim)
  232.      long partArrIndex;
  233.      long numParts;
  234.      PCMDDerivatives * baseDerivs;
  235.     PCMDDerivatives * theseDs;
  236.     double delT;
  237.     double delTSquOverTwo;
  238.     double delTCubeOverSix;
  239.     double delTFourthOverTwentyFour;
  240.     double delTFifthOverOneTwenty;
  241.     kSanVector  oldFourthDif;
  242.     kSanVector  oldThirdDif;
  243.     kSanVector  oldAccel;
  244.     kSanVector  oldVel;
  245.     baseDerivs = PCMDLockDerivs( pcmd);
  246.     
  247.      delT =  *delTPtr ;
  248.      delTSquOverTwo = delT * delT * 0.5;
  249.      delTCubeOverSix = delTSquOverTwo * delT * 0.3333333333333;
  250.      delTFourthOverTwentyFour = delTCubeOverSix * delT * 0.25;
  251.      delTFifthOverOneTwenty = delTFourthOverTwentyFour * delT * 0.2;
  252.      dispatchCount(OHOCtr(pcmdObj), ClassParticle, &numParts);
  253.     partArrIndex = 0;
  254.     while (partArrIndex< numParts)
  255.     {
  256.          theseDs = &(baseDerivs[partArrIndex]);
  257.         oldFourthDif = theseDs->fourthD;
  258.         oldThirdDif = theseDs->thirdD;
  259.         oldAccel = theseDs->accel;
  260.         oldVel = basePart->baseVelocity[partArrIndex];
  261. // save oldPosition for gear group management
  262.         theseDs->oldPosition  =  basePart->basePosition[partArrIndex];
  263.         
  264. //predict fourth differential
  265.         theseDs->fourthD.a = oldFourthDif.a + theseDs->fifthD.a * delT;
  266.         theseDs->fourthD.b = oldFourthDif.b + theseDs->fifthD.b * delT;
  267. #ifndef __2DVectors__
  268.         theseDs->fourthD.c = oldFourthDif.c + theseDs->fifthD.c * delT;
  269. #endif        
  270. //predict third differential
  271.         theseDs->thirdD.a = oldThirdDif.a + 
  272.                 oldFourthDif.a * delT + 
  273.                 theseDs->fifthD.a * delTSquOverTwo;
  274.         theseDs->thirdD.b = oldThirdDif.b + 
  275.                 oldFourthDif.b * delT + 
  276.                 theseDs->fifthD.b * delTSquOverTwo;
  277. #ifndef __2DVectors__
  278.         theseDs->thirdD.c = oldThirdDif.c + 
  279.                 oldFourthDif.c * delT + 
  280.                 theseDs->fifthD.c * delTSquOverTwo;
  281. #endif        
  282.         
  283. //predict second differential
  284.         theseDs->accel.a = oldAccel.a + 
  285.                 oldThirdDif.a * delT + 
  286.                 oldFourthDif.a * delTSquOverTwo + 
  287.                 theseDs->fifthD.a * delTCubeOverSix;
  288.         theseDs->accel.b = oldAccel.b + 
  289.                 oldThirdDif.b * delT + 
  290.                 oldFourthDif.b * delTSquOverTwo + 
  291.                 theseDs->fifthD.b * delTCubeOverSix;
  292. #ifndef __2DVectors__
  293.         theseDs->accel.c = oldAccel.c + 
  294.                 oldThirdDif.c * delT + 
  295.                 oldFourthDif.c * delTSquOverTwo + 
  296.                 theseDs->fifthD.c * delTCubeOverSix;
  297. #endif        
  298.  
  299. //predict first differential
  300.         basePart->baseVelocity[partArrIndex].a = oldVel.a + 
  301.                 oldAccel.a * delT + 
  302.                 oldThirdDif.a * delTSquOverTwo + 
  303.                 oldFourthDif.a * delTCubeOverSix + 
  304.                 theseDs->fifthD.a * delTFourthOverTwentyFour;
  305.         basePart->baseVelocity[partArrIndex].b = oldVel.b + 
  306.                 oldAccel.b * delT + 
  307.                 oldThirdDif.b * delTSquOverTwo + 
  308.                 oldFourthDif.b * delTCubeOverSix + 
  309.                 theseDs->fifthD.b * delTFourthOverTwentyFour;
  310. #ifndef __2DVectors__
  311.         basePart->baseVelocity[partArrIndex].c = oldVel.c + 
  312.                 oldAccel.c * delT + 
  313.                 oldThirdDif.c * delTSquOverTwo + 
  314.                 oldFourthDif.c * delTCubeOverSix + 
  315.                 theseDs->fifthD.c * delTFourthOverTwentyFour;
  316. #endif        
  317.                 
  318.  
  319. //predict position
  320.         basePart->basePosition[partArrIndex].a = theseDs->oldPosition.a + 
  321.                 oldVel.a * delT + 
  322.                 oldAccel.a * delTSquOverTwo + 
  323.                 oldThirdDif.a * delTCubeOverSix + 
  324.                 oldFourthDif.a * delTFourthOverTwentyFour + 
  325.                 theseDs->fifthD.a * delTFifthOverOneTwenty;
  326.         basePart->basePosition[partArrIndex].b = theseDs->oldPosition.b + 
  327.                 oldVel.b * delT + 
  328.                 oldAccel.b * delTSquOverTwo + 
  329.                 oldThirdDif.b * delTCubeOverSix + 
  330.                 oldFourthDif.b * delTFourthOverTwentyFour + 
  331.                 theseDs->fifthD.b * delTFifthOverOneTwenty;
  332. #ifndef __2DVectors__
  333.         basePart->basePosition[partArrIndex].c = theseDs->oldPosition.c + 
  334.                 oldVel.c * delT + 
  335.                 oldAccel.c * delTSquOverTwo + 
  336.                 oldThirdDif.c * delTCubeOverSix + 
  337.                 oldFourthDif.c * delTFourthOverTwentyFour + 
  338.                 theseDs->fifthD.c * delTFifthOverOneTwenty;
  339. #endif        
  340.         ++partArrIndex;
  341.     }
  342.      PCMDReleaseDerivs( pcmd);
  343. }
  344.  
  345. short    PCMDDoCorrectionFreeGroups(kozoObject *pcmdObj, PCMDMethod *pcmd, kSanSimulation *sim, particle *basePart,  kSanGroup *gp, double *delTPtr);
  346. short    PCMDDoCorrectionFreeGroups(kozoObject *pcmdObj, PCMDMethod *pcmd, kSanSimulation *sim, particle *basePart,  kSanGroup *gp, double *delTPtr)
  347. {
  348. #pragma unused(pcmdObj)
  349.      long i;
  350.      long count;
  351.      long partArrIndex;
  352.      long *list;
  353.      char largeCorrection = false;
  354.     PCMDDerivatives *theseDs;
  355.     PCMDDerivatives *baseDerivs;
  356.     kSanAtomtype **baseAT;
  357.     double thisFactor;
  358.     double delT;
  359.     double inverseMass;
  360.     double inverseDelT;
  361.     double delTSquOverTwo;
  362.     kSanVector corrFac ;  // correctionFactor delR2 in X, Y, Z
  363.      
  364.     delT = (*delTPtr);
  365.     inverseDelT = 1.0 / delT ;
  366.     delTSquOverTwo =  delT * delT * 0.5;
  367.     baseAT = simLockAtomTypes(sim);
  368.  
  369.     baseDerivs = PCMDLockDerivs( pcmd);
  370.     count = OHListCount(&gp->particles);
  371.     list = lockList(&gp->particles);
  372.     for(i=0;i<count;++i)
  373.     {
  374.         partArrIndex = list[i];
  375.          theseDs = &(baseDerivs[partArrIndex]);
  376.         inverseMass =    baseAT[BPNthType(basePart, partArrIndex)]->specs.inverseMass;
  377.  
  378.         corrFac.a = (basePart->baseForce[partArrIndex].a * inverseMass) -  theseDs->accel.a ; // accel was predicted
  379.         corrFac.b = (basePart->baseForce[partArrIndex].b * inverseMass) -  theseDs->accel.b ;
  380. #ifndef __2DVectors__
  381.         corrFac.c = (basePart->baseForce[partArrIndex].c * inverseMass) -  theseDs->accel.c ;
  382. #endif
  383.         multkSanVectorD (&corrFac, delTSquOverTwo, &corrFac );
  384.   
  385.         basePart->basePosition[partArrIndex].a = basePart->basePosition[partArrIndex].a + (pcmd->alpha.nought * corrFac.a);
  386.         basePart->basePosition[partArrIndex].b = basePart->basePosition[partArrIndex].b + (pcmd->alpha.nought * corrFac.b);
  387.  #ifndef __2DVectors__
  388.        basePart->basePosition[partArrIndex].c = basePart->basePosition[partArrIndex].c + (pcmd->alpha.nought * corrFac.c);
  389.  #endif
  390.  
  391.  
  392. #ifndef __2DVectors__
  393.         if ( (fabs(corrFac.a) + fabs(corrFac.b) + fabs (corrFac.c))  > 0.1 ) 
  394. #else 
  395.         if ( (fabs(corrFac.a) + fabs(corrFac.b) )  > 0.1 ) 
  396. #endif
  397.         {
  398.              largeCorrection = true;
  399.           }
  400.           
  401.         multkSanVectorD (&corrFac, inverseDelT, &corrFac );
  402.  
  403.       basePart->baseVelocity[partArrIndex].a = basePart->baseVelocity[partArrIndex].a + (pcmd->alpha.one * corrFac.a);
  404.       basePart->baseVelocity[partArrIndex].b = basePart->baseVelocity[partArrIndex].b + (pcmd->alpha.one * corrFac.b);
  405.  
  406. #ifndef __2DVectors__
  407.       basePart->baseVelocity[partArrIndex].c = basePart->baseVelocity[partArrIndex].c + (pcmd->alpha.one * corrFac.c);
  408. #endif
  409.  
  410.   // correctAccel
  411.         thisFactor = 2.0 * inverseDelT;
  412.         multkSanVectorD (&corrFac, thisFactor, &corrFac );
  413.  
  414.       theseDs->accel.a = theseDs->accel.a + (pcmd->alpha.two * corrFac.a);
  415.       theseDs->accel.b = theseDs->accel.b + (pcmd->alpha.two * corrFac.b);
  416. #ifndef __2DVectors__
  417.       theseDs->accel.c = theseDs->accel.c + (pcmd->alpha.two * corrFac.c);
  418. #endif
  419.   // correctThirdDif
  420.         thisFactor = 3.0 * inverseDelT;
  421.         multkSanVectorD (&corrFac, thisFactor, &corrFac );
  422.       theseDs->thirdD.a = theseDs->thirdD.a + (pcmd->alpha.three * corrFac.a);
  423.       theseDs->thirdD.b = theseDs->thirdD.b + (pcmd->alpha.three * corrFac.b);
  424.  #ifndef __2DVectors__
  425.      theseDs->thirdD.c = theseDs->thirdD.c + (pcmd->alpha.three * corrFac.c);
  426. #endif
  427.   // correctFourthDif
  428.        thisFactor = 4.0 * inverseDelT;
  429.         multkSanVectorD (&corrFac, thisFactor, &corrFac );
  430.       theseDs->fourthD.a = theseDs->fourthD.a + (pcmd->alpha.four * corrFac.a);
  431.       theseDs->fourthD.b = theseDs->fourthD.b + (pcmd->alpha.four * corrFac.b);
  432. #ifndef __2DVectors__
  433.       theseDs->fourthD.c = theseDs->fourthD.c + (pcmd->alpha.four * corrFac.c);
  434. #endif
  435.   // correctFifthDif
  436.        thisFactor = 5.0 * inverseDelT;
  437.         multkSanVectorD (&corrFac, thisFactor, &corrFac );
  438.       theseDs->fifthD.a = 0.5 * theseDs->fifthD.a + (pcmd->alpha.five * corrFac.a);
  439.       theseDs->fifthD.b = 0.5 * theseDs->fifthD.b + (pcmd->alpha.five * corrFac.b);
  440. #ifndef __2DVectors__
  441.       theseDs->fifthD.c = 0.5 * theseDs->fifthD.c + (pcmd->alpha.five * corrFac.c);
  442. #endif
  443.    }
  444.    PCMDReleaseDerivs( pcmd);
  445.    releaseList(&gp->particles);
  446.    simReleaseAtomTypes(sim);
  447.     return(largeCorrection);
  448. }
  449. void     PCMDDoCorrection(kozoObject *pcmdObj, PCMDMethod *pcmd, kSanSimulation *sim, particle *basePart,  kSanGroup **baseGroup, long numGroups, double  *delT)
  450. {
  451.     short err = noErr;
  452.     long i;
  453.     kSanGroup *gp;
  454.  
  455.     for(i=0; i<numGroups ;++i)
  456.     {
  457.         gp = baseGroup[i];
  458.  
  459.         if ((gp->XYZConstraints[0] == kGroupMovesFreely)  &&
  460.             (gp->XYZConstraints[1] == kGroupMovesFreely) 
  461. #ifndef __2DVectors__
  462.              &&  (gp->XYZConstraints[2] == kGroupMovesFreely)  
  463. #endif
  464.              
  465.                                                               ) // totally free
  466.         {
  467.             err += PCMDDoCorrectionFreeGroups(pcmdObj,pcmd,sim, basePart, gp, delT);  // no constraints = empty function
  468.         }
  469.         else if  ((gp->XYZConstraints[0] == kGroupIsFixed)  &&
  470.                    (gp->XYZConstraints[1] == kGroupIsFixed)
  471. #ifndef __2DVectors__
  472.                      && (gp->XYZConstraints[2] == kGroupIsFixed)  
  473. #endif
  474.                                                             ) // fixed 
  475.         {
  476.             err +=PCMDDoCorrectionFixedGroups(pcmdObj,pcmd,sim, basePart, gp, delT);
  477.         }
  478.         else if  ((gp->XYZConstraints[0] != kGroupMovesFreely)  &&
  479.             (gp->XYZConstraints[1] != kGroupMovesFreely) 
  480. #ifndef __2DVectors__
  481.                  && (gp->XYZConstraints[2] != kGroupMovesFreely)
  482. #endif
  483.                                                               ) // moves as a block in 1 2 or 3 axes
  484.         {
  485.             err +=PCMDDoCorrectionBlockGroups(pcmdObj,pcmd,sim, basePart, gp, delT);
  486.         }
  487.         else  // some other combination of fixed, block and free
  488.         {
  489.             err +=PCMDDoCorrectionMixedGroups(pcmdObj,pcmd,sim, basePart, gp, delT);                
  490.         }
  491.     }
  492.     if (err != 0)
  493.     {
  494.         OHSystemBeep();
  495.          sim->deltaT *= 0.5;
  496.     }
  497. }
  498. short PCMDDoCorrectionBlockGroups(kozoObject *pcmdObj, PCMDMethod *pcmd, kSanSimulation *sim, particle *basePart,  kSanGroup *gp, double  *delTPtr)
  499. { // each axis eith moves as a group or is fixed, but motion is allowed in at least one direction
  500. #pragma unused(pcmdObj)
  501.      long i;
  502.     long count;
  503.      long partArrIndex;
  504.      long *list;
  505.      char largeCorrection = false;
  506.      PCMDDerivatives *theseDs;
  507.     PCMDDerivatives *baseDerivs;
  508.     kSanAtomtype **baseAT;
  509.     double delT;
  510.     double inverseDelT;
  511.    kSanVector corrFac ;  // correctionFactor delR2 in X
  512.   //double corrFacB;  // correctionFactor delR2 in Y
  513.   // double corrFacC;  // correctionFactor delR2 in Z
  514.    double netMass;
  515.    kSanVector  netForce;
  516.    kSanVector  realAccel;
  517.    kSanVector  netAccel;
  518.    kSanVector  netMoment;
  519.    kSanVector  netVelocity;
  520.     kSanVector  netThirdD;
  521.     kSanVector  netFourthD;
  522.     kSanVector  netFifthD;
  523.     kSanVector  thirdDiff;
  524.     kSanVector  fourthDiff;
  525.     kSanVector  fifthDiff;
  526.     kSanVector  displacement;
  527.    
  528.     delT = (*delTPtr);
  529.     inverseDelT = 1.0 / delT ;
  530.  
  531.     netMass = 0;
  532.     zerokSanVectorD(& fifthDiff);
  533.     zerokSanVectorD(& fourthDiff);
  534.     zerokSanVectorD(& thirdDiff);
  535.     zerokSanVectorD(& netAccel);
  536.     zerokSanVectorD(& netForce);
  537.     zerokSanVectorD(& netMoment);
  538.     zerokSanVectorD(& netFifthD);
  539.     zerokSanVectorD(& netFourthD);
  540.     zerokSanVectorD(& netThirdD);
  541.  
  542.     baseAT = simLockAtomTypes(sim);
  543.     baseDerivs = PCMDLockDerivs( pcmd);
  544.     count = OHListCount(&gp->particles);
  545.     list = lockList(&gp->particles);
  546.     for(i=0;i<count;++i)
  547.     {
  548.         double thisMass ;
  549.         partArrIndex = list[i];
  550.         thisMass = baseAT[BPNthType(basePart, partArrIndex)]->specs.mass;
  551.         theseDs = &(baseDerivs[partArrIndex]);
  552.         netMass += thisMass; // sum masses, forces and moments
  553.         addkSanVectorsD (&netFifthD, &theseDs->fifthD, &netFifthD );
  554.         addkSanVectorsD (&netFourthD, &theseDs->fourthD, &netFourthD );
  555.         addkSanVectorsD (&netThirdD, &theseDs->thirdD, &netThirdD );
  556.         addkSanVectorsD (&netAccel, &theseDs->accel, &netAccel );
  557.         addkSanVectorsD (&netForce, &basePart->baseForce[partArrIndex], &netForce ); 
  558.  
  559.         netMoment.a += basePart->baseVelocity[partArrIndex].a * thisMass;
  560.         netMoment.b += basePart->baseVelocity[partArrIndex].b * thisMass;
  561. #ifndef __2DVectors__
  562.         netMoment.c += basePart->baseVelocity[partArrIndex].c * thisMass;
  563. #endif
  564.     }
  565.     {
  566.         double inverseNetMass;
  567.         double inverseCount;
  568.         inverseNetMass = 1.0 / netMass;
  569.         inverseCount = 1.0 / ((double)count);
  570.         multkSanVectorD(&netForce, inverseNetMass, &realAccel);
  571.         multkSanVectorD(&netMoment, inverseNetMass, &netVelocity);
  572.         multkSanVectorD(&netAccel, inverseCount, &netAccel);
  573.         multkSanVectorD(&netThirdD, inverseCount, &netThirdD);
  574.         multkSanVectorD(&netFourthD, inverseCount, &netFourthD);
  575.         multkSanVectorD(&netFifthD, inverseCount, &netFifthD);
  576.     }    
  577.     {
  578.         subkSanVectorsD (&realAccel, &netAccel,  &corrFac );
  579.         //corrFacA = realAccel.a -  netAccel.a ; 
  580.         //corrFacB = realAccel.b -  netAccel.b ; 
  581.         //corrFacC = realAccel.c -  netAccel.c ;
  582.         {
  583.             double delTSquOverTwo ;
  584.             delTSquOverTwo = delT * delT * 0.5;
  585.             multkSanVectorD(&corrFac, delTSquOverTwo, &corrFac);
  586.  
  587.  
  588. #ifndef __2DVectors__
  589.                if ( (fabs(corrFac.a) + fabs(corrFac.b) + fabs(corrFac.c))  > 0.1 ) 
  590. #else
  591.                if ( (fabs(corrFac.a) + fabs(corrFac.b) )  > 0.1 ) 
  592. #endif
  593.                {
  594.                   largeCorrection = true;
  595.                }
  596.                
  597.          }
  598.         displacement.a = pcmd->alpha.nought * corrFac.a *
  599.                         ((gp->XYZConstraints[0] == kGroupIsFixed) ? 0 : 1);  // tricky: this is zero if fixed, one if a block
  600.         displacement.b = pcmd->alpha.nought * corrFac.b *
  601.                         ((gp->XYZConstraints[1] == kGroupIsFixed) ? 0 : 1);  // tricky: this is zero if fixed, one if a block
  602. #ifndef __2DVectors__
  603.         displacement.c = pcmd->alpha.nought * corrFac.c *
  604.                         ((gp->XYZConstraints[2] == kGroupIsFixed) ? 0 : 1);  // tricky: this is zero if fixed, one if a block
  605. #endif
  606.         
  607.          multkSanVectorD(&corrFac, inverseDelT, &corrFac);
  608.  
  609.          netVelocity.a = (basePart->baseVelocity[partArrIndex].a + (pcmd->alpha.one * corrFac.a)) *
  610.                           gp->XYZConstraints[0];    
  611.           netVelocity.b = (basePart->baseVelocity[partArrIndex].b + (pcmd->alpha.one * corrFac.b)) *
  612.                           gp->XYZConstraints[1];    
  613. #ifndef __2DVectors__
  614.          netVelocity.c = (basePart->baseVelocity[partArrIndex].c + (pcmd->alpha.one * corrFac.c)) *
  615.                           gp->XYZConstraints[2];    
  616. #endif
  617.  
  618.          multkSanVectorD(&corrFac, (6.0 * inverseDelT * inverseDelT), &corrFac);
  619.          multAddkSanVectorsD(&thirdDiff, &corrFac, pcmd->alpha.three, &thirdDiff);
  620.          addkSanVectorsD(&thirdDiff, &netThirdD, &thirdDiff);
  621.  
  622.          multkSanVectorD(&corrFac, (4.0 * inverseDelT), &corrFac);
  623.          multAddkSanVectorsD(&fourthDiff, &corrFac, pcmd->alpha.four, &fourthDiff);
  624.          addkSanVectorsD(&fourthDiff, &netFourthD, &fourthDiff);
  625.  
  626.          multkSanVectorD(&corrFac, (5.0 * inverseDelT), &corrFac);
  627.          multAddkSanVectorsD(&fifthDiff, &corrFac, pcmd->alpha.five, &fifthDiff);
  628.          addkSanVectorsD(&fifthDiff, &netFifthD, &fifthDiff);
  629.       }
  630.     
  631.     for(i=0;i<count;++i)
  632.     // loop over particles
  633.     {
  634.          theseDs = &(baseDerivs[partArrIndex]);
  635.        addkSanVectorsD (&basePart->basePosition[partArrIndex], &displacement, &basePart->basePosition[partArrIndex]);  
  636.  
  637.        basePart->baseVelocity[partArrIndex] = netVelocity;
  638.       theseDs->accel = realAccel;
  639.       theseDs->thirdD = thirdDiff;
  640.       theseDs->fourthD = fourthDiff;
  641.       theseDs->fifthD = fifthDiff;
  642.    }
  643.    simReleaseAtomTypes(sim);
  644.     PCMDReleaseDerivs( pcmd);
  645.     return(largeCorrection);
  646.  
  647. }
  648. short PCMDDoCorrectionFixedGroups(kozoObject *pcmdObj, PCMDMethod *pcmd, kSanSimulation *sim, particle *basePart,  kSanGroup *gp, double  *delTPtr)
  649. {
  650.  #pragma unused(sim)
  651. #pragma unused(pcmdObj)
  652. #pragma unused(delTPtr)
  653.     long i;
  654.      long count;
  655.      long partArrIndex;
  656.      long *list;
  657.      kSanVector  zeroVec;
  658.     PCMDDerivatives *theseDs;
  659.      PCMDDerivatives *baseDerivs;
  660.    
  661.     zerokSanVectorD(&zeroVec);
  662.  
  663.     baseDerivs = PCMDLockDerivs( pcmd);
  664.     count = OHListCount(&gp->particles);
  665.     list = lockList(&gp->particles);
  666.     for(i=0;i<count;++i)
  667.     {
  668.         partArrIndex = list[i];
  669.          theseDs = &(baseDerivs[partArrIndex]);
  670.     
  671.  
  672.         basePart->basePosition[partArrIndex] = theseDs->oldPosition;
  673.         basePart->baseVelocity[partArrIndex] = zeroVec;
  674.         theseDs->accel = zeroVec;
  675.         theseDs->thirdD = zeroVec;
  676.         theseDs->fourthD = zeroVec;
  677.         theseDs->fifthD = zeroVec;
  678.    }
  679.     PCMDReleaseDerivs( pcmd);
  680.    return(noErr);
  681. }                
  682. short PCMDDoCorrectionMixedGroups(kozoObject *pcmdObj, PCMDMethod *pcmd, kSanSimulation *sim, particle *basePart,  kSanGroup *gp, double  *delTPtr)
  683. {
  684. #pragma unused(pcmdObj)
  685.      long j;
  686.     long i;
  687.     long partArrIndex;
  688.     long count;
  689.     long *list;
  690.      char largeCorrection = false;
  691.     PCMDDerivatives *theseDs;
  692.     PCMDDerivatives *baseDerivs;
  693.     kSanAtomtype **baseAT;
  694.     double delT;
  695.     double netMass;
  696.     double netAcc;
  697.     double delTSquOverTwo;
  698.     double inverseDelT;
  699.     double corrFac;
  700.     netMass = 0;
  701.     
  702.      
  703.     delT = (*delTPtr);
  704.     inverseDelT = 1.0 / delT ;
  705.     delTSquOverTwo =  delT * delT * 0.5;
  706.     baseAT = simLockAtomTypes(sim);
  707.     baseDerivs = PCMDLockDerivs( pcmd);
  708.     count = OHListCount(&gp->particles);
  709.     list = lockList(&gp->particles);
  710.      for(j=0; j<NUMDIMENSIONS;++j)
  711.     {
  712.         netAcc = 0;
  713.         switch (gp->XYZConstraints[j])
  714.         {
  715.             case kGroupIsFixed:
  716.                 for(i=0;i<count;++i)
  717.                 {        
  718.                     long partArrIndex;    
  719.                     partArrIndex = list[i];
  720.                     
  721.                     theseDs  = &(baseDerivs[partArrIndex]);
  722.                     ((double *)&(basePart->basePosition[partArrIndex]))[j] = ((double *)&(theseDs->oldPosition))[j];
  723.                     ((double *)&(basePart->baseVelocity[partArrIndex]))[j] = 0;
  724.                     ((double *)&(theseDs->accel))[j] = 0;
  725.                     ((double *)&(theseDs->thirdD))[j] = 0;
  726.                     ((double *)&(theseDs->fourthD))[j] = 0;
  727.                     ((double *)&(theseDs->fifthD))[j] = 0;
  728.                 }                                    
  729.                 break;
  730.             case kGroupMovesAsABlock:
  731.                 if (netMass == 0)  // add up the mass if we haven't already
  732.                 {
  733.                     for(i=0;i<count;++i)
  734.                     {
  735.                         partArrIndex = list[i];
  736.                         netMass += baseAT[BPNthType(basePart, partArrIndex)]->specs.mass;
  737.                     }
  738.                 }
  739.                 for(i=0;i<count;++i)
  740.                 {
  741.                     partArrIndex = list[i];
  742.                     netAcc +=  ((double *)&(basePart->baseForce[partArrIndex]))[j];  
  743.                 }
  744.                 netAcc *= (delT / netMass) ; 
  745.                 for(i=0;i<count;++i)
  746.                 {        
  747.                     long partArrIndex;    
  748.                     partArrIndex = list[i];
  749.                      theseDs = &(baseDerivs[partArrIndex]);
  750.                     corrFac = netAcc - ((double *)&(theseDs->accel))[j] ;
  751.                     ((double *)&(theseDs->accel))[j] = netAcc;
  752.             
  753.                     corrFac *= delTSquOverTwo;
  754.                      if ( fabs(corrFac) > 0.05 ) largeCorrection = true;
  755.  
  756.               // correct position
  757.                   ((double *)&(basePart->basePosition[partArrIndex]))[j] += (pcmd->alpha.nought * corrFac);
  758.               // correctVelocity
  759.                     corrFac *= inverseDelT;
  760.                   ((double *)&(basePart->baseVelocity[partArrIndex]))[j] += (pcmd->alpha.one * corrFac);
  761.               // correctThirdDif
  762.                     corrFac *= 6.0 * inverseDelT * inverseDelT;
  763.                   ((double *)&(theseDs->thirdD))[j] += (pcmd->alpha.three * corrFac);
  764.               // correctFourthDif
  765.                     corrFac *= 4.0 * inverseDelT;
  766.                   ((double *)&(theseDs->fourthD))[j] += (pcmd->alpha.four * corrFac);
  767.               // correctFifthDif
  768.                     corrFac *= 5.0 * inverseDelT;
  769.                   ((double *)&(theseDs->fifthD))[j] += (pcmd->alpha.five * corrFac);                                
  770.                 }            
  771.                 break;
  772.                 
  773.             case kGroupMovesFreely:
  774.                 for(i=0;i<count;++i)
  775.                 {
  776.                     double inverseMass;
  777.                     double realAcc;
  778.                      partArrIndex = list[i];
  779.                     theseDs = &(baseDerivs[i]);
  780.                     inverseMass =   baseAT[BPNthType(basePart, partArrIndex)]->specs.inverseMass; 
  781.                     realAcc = ((double *)&(basePart->baseForce[partArrIndex]))[j] * inverseMass;
  782.                     corrFac = realAcc - ((double *)&(theseDs->accel))[j] ;
  783.                     
  784.                     
  785.                     ((double *)&(theseDs->accel))[j] = realAcc;
  786.             
  787.                     corrFac *= delTSquOverTwo;
  788.                     if ( fabs(corrFac) > 0.05 ) largeCorrection = true;
  789.  
  790.               // correct position
  791.                   ((double *)&(basePart->basePosition[partArrIndex]))[j] += (pcmd->alpha.nought * corrFac);
  792.               // correctVelocity
  793.                     corrFac *= inverseDelT;
  794.                   ((double *)&(basePart->baseVelocity[partArrIndex]))[j] += (pcmd->alpha.one * corrFac);
  795.               // correctThirdDif
  796.                     corrFac *= 6.0 * inverseDelT * inverseDelT;
  797.                   ((double *)&(theseDs->thirdD))[j] += (pcmd->alpha.three * corrFac);
  798.               // correctFourthDif
  799.                     corrFac *= 4.0 * inverseDelT;
  800.                   ((double *)&(theseDs->fourthD))[j] += (pcmd->alpha.four * corrFac);
  801.               // correctFifthDif
  802.                     corrFac *= 5.0 * inverseDelT;
  803.                   ((double *)&(theseDs->fifthD))[j] += (pcmd->alpha.five * corrFac);                                
  804.                 }                    
  805.                 break;
  806.         }
  807.     }
  808.      simReleaseAtomTypes(sim);
  809.     PCMDReleaseDerivs( pcmd);
  810.    return(largeCorrection);
  811. }
  812.  
  813. short PCMDIteration ( kSanMethodPlugIn *mpi,  kSanSimulation *sim) 
  814. {
  815.     short err = noErr;
  816. //    long numParts;
  817.     PCMDMethod *pcmd = mpi->methodPlugPrivateData;
  818.     particle *basePart;
  819.     double delT;
  820.  
  821.     basePart = simLockParts(sim);
  822.     delT =  sim->deltaT;
  823. //     numParts = simGetNumParts(sim);
  824.      if (pcmd->dirtyDerivs) 
  825.      {
  826.           pcmdCalcInitialDerivs(mpi->mpiObj,pcmd, sim, basePart, &delT);  
  827.      }
  828.      else
  829.      {
  830.           normalIteration(mpi->mpiObj,pcmd, sim, basePart, &delT);
  831.     }
  832.     simIncrementStateCount(sim);    
  833.     simReleaseParts(sim);
  834.     return (noErr);
  835. }
  836. void    normalIteration(kozoObject *pcmdObj, PCMDMethod *pcmd, kSanSimulation *sim, particle *basePart, double  *delTPtr)
  837. {
  838.     short err = noErr;
  839.     long numGroups;
  840.     kSanGroup **baseGroup;
  841.      numGroups = simGetNumGroups(sim);
  842.     baseGroup = simLockGroups(sim);    
  843.         PCMDDoPrediction(pcmdObj,pcmd, sim, basePart, delTPtr);  // prediction
  844.         err = simDoProgressFunction (sim);
  845.  
  846.         if (!err) err = calculateForce(sim); 
  847.         if (!err) err = simDoProgressFunction (sim);
  848.         if (!err) PCMDDoCorrection(pcmdObj,pcmd, sim, basePart, baseGroup, numGroups, delTPtr);  // prediction
  849.     simReleaseGroups(sim);
  850.  }
  851.